%系统建模分析
clear;

L0s=0.11:0.04:0.35; % L0变化范围

Ks=zeros(4,10,length(L0s),length(L0s)); % 存放不同L0对应的K

for l_li=1:length(L0s)
    for l_ri=1:length(L0s)
        fprintf('l_li=%d,l_ri=%d\n', l_li, l_ri);
        %定义状态量
        syms s s_dot phi phi_dot theta_ll theta_dot_ll theta_lr theta_dot_lr theta_b theta_dot_b
        %定义控制量
        syms T_lwl T_lwr T_bll T_blr
        
        %状态向量
        x = [s s_dot phi phi_dot theta_ll theta_dot_ll theta_lr theta_dot_lr theta_b theta_dot_b].';
        %控制向量
        u = [T_lwl T_lwr T_bll T_blr].';
        
        %定义变量
        syms theta_ll theta_lr
        syms theta_ddot_b
        syms theta_ddot_wl theta_ddot_wr
        syms theta_ddot_ll theta_ddot_lr
        
        %定义参数(使用国际单位制)
        %syms I_b I_z I_w I_ll I_lr
        %syms l_c l_l l_r l_bl l_br l_wl l_wr
        %syms m_w m_l m_b
        %syms R_w R_l
        %syms g
        
        %m/s^2
        g = 9.8;
        
        %m
        R_w = 0.106; R_l = 0.51984/2;
        l_l = L0s(l_li);l_r = L0s(l_ri);
        l_bl = 0.218*l_l + 0.075;
        l_wl = l_l - l_bl;
        l_br = 0.218*l_r + 0.075;
        l_wr = l_r - l_br;
        l_c = 0.02757;
    
        %kg
        m_w = 1.76871;
        m_l = 0.46492*2+0.34456+0.4212;
        m_b = 7.92144;
    
        %kg*m^2
        I_w = 10330043.81e-9;
        I_ll = 0.4*l_l+0.07;
        I_lr = 0.4*l_r+0.07;
        I_b = 123284905.64e-9;
        I_z = 668422910.13e-9;
        
        %消去约束力，并对腿部、机体倾角进行小角度近似，得到如下方程组
        eq1 = (I_w*l_l/R_w + m_w*R_w*l_l + m_l*R_w*l_bl)*theta_ddot_wl ...
             +(m_l*l_wl*l_bl - I_ll)*theta_ddot_ll ...
             +(m_l*l_wl + 1/2*m_b*l_l)*g*theta_ll ...
             +T_bll - T_lwl*(1+l_l/R_w) == 0;
        
        eq2 = (I_w*l_r/R_w + m_w*R_w*l_r + m_l*R_w*l_br)*theta_ddot_wr ...
             +(m_l*l_wr*l_br - I_lr)*theta_ddot_lr ...
             +(m_l*l_wr + 1/2*m_b*l_r)*g*theta_lr ...
             +T_blr - T_lwr*(1+l_r/R_w) == 0;
        
        eq3 = -(m_w*R_w^2 + I_w + m_l*R_w^2 + 1/2*m_b*R_w^2)*theta_ddot_wl ...
              -(m_w*R_w^2 + I_w + m_l*R_w^2 + 1/2*m_b*R_w^2)*theta_ddot_wr ...
              -(m_l*R_w*l_wl + 1/2*m_b*R_w*l_l)*theta_ddot_ll ...
              -(m_l*R_w*l_wr + 1/2*m_b*R_w*l_r)*theta_ddot_lr ...
              +T_lwl + T_lwr == 0;
        
        eq4 = (m_w*R_w*l_c + I_w*l_c/R_w + m_l*R_w*l_c)*theta_ddot_wl ...
             +(m_w*R_w*l_c + I_w*l_c/R_w + m_l*R_w*l_c)*theta_ddot_wr ...
             + m_l*l_wl*l_c*theta_ddot_ll ...
             + m_l*l_wr*l_c*theta_ddot_lr ...
             - I_b*theta_ddot_b + m_b*g*l_c*theta_b ...
             -(T_lwl + T_lwr)*l_c/R_w - (T_bll + T_blr) == 0;
        
        eq5 = (1/2*I_z*R_w/R_l + I_w*R_l/R_w)*theta_ddot_wl ...
             -(1/2*I_z*R_w/R_l + I_w*R_l/R_w)*theta_ddot_wr ...
             + 1/2*I_z*l_l/R_l*theta_ddot_ll ...
             - 1/2*I_z*l_r/R_l*theta_ddot_lr ...
             - T_lwl*R_l/R_w + T_lwr*R_l/R_w == 0;
        
        %求解5个广义坐标的二阶导数
        [theta_ddot_b,theta_ddot_wl,theta_ddot_wr,theta_ddot_ll,theta_ddot_lr]= ...
            solve([eq1,eq2,eq3,eq4,eq5],[theta_ddot_b,theta_ddot_wl,theta_ddot_wr,theta_ddot_ll,theta_ddot_lr]);
        
        syms s_ddot phi_ddot
        x_dot = [s_dot s_ddot phi_dot phi_ddot theta_dot_ll theta_ddot_ll theta_dot_lr theta_ddot_lr theta_dot_b theta_ddot_b].';
        
        Ja=jacobian(x_dot,x);
        Jb=jacobian(x_dot,u);
        
        %构建状态矩阵
        A=Ja;
        for j = 5:2:9
            A(2,j)=R_w/2*(diff(theta_ddot_wl,x(j))+diff(theta_ddot_wr,x(j)));
        end
        
        for j = 5:2:9
            A(4,j)=R_w/R_l/2*(-diff(theta_ddot_wl,x(j))+diff(theta_ddot_wr,x(j)))...
                  +l_l/R_l/2*(diff(theta_ddot_lr,x(j))-diff(theta_ddot_ll,x(j)));
        end
        
        %构建输入矩阵
        B=Jb;
        for j = 1:1:4
            B(2,j)=R_w/2*(diff(theta_ddot_wl,u(j))+diff(theta_ddot_wr,u(j)));
        end
        
        for j = 1:1:4
            B(4,j)=R_w/R_l/2*(-diff(theta_ddot_wl,x(j))+diff(theta_ddot_wr,x(j)))...
                  +l_l/R_l/2*(diff(theta_ddot_lr,x(j))-diff(theta_ddot_ll,x(j)));
        end
    
        % 定义权重矩阵Q, R
        Q_s            = 1;
        Q_s_dot        = 10;
        Q_phi          = 50;
        Q_phi_dot      = 20;
        Q_theta_ll     = 500;
        Q_theta_dot_ll = 100;
        Q_theta_lr     = 500;
        Q_theta_dot_lr = 100;
        Q_theta_b      = 500;
        Q_theta_dot_b  = 500;
        Q=diag([Q_s Q_s_dot Q_phi Q_phi_dot Q_theta_ll Q_theta_dot_ll Q_theta_lr Q_theta_dot_lr Q_theta_b Q_theta_dot_b]);
        R=diag([1 1 1 1]);
        
        A = eval(A);
        B = eval(B);

        % 求解反馈矩阵K
        Ks(:,:,l_li,l_ri) = lqr(A,B,Q,R);
    end
end

% 对K的每个元素关于l_l和l_r进行拟合
K=sym('K',[4 10]);
syms l_l l_r;

for i=1:4
    for j=1:10
        %p=polyfit(L0s,reshape(Ks(x,y,:),1,length(L0s)),3);
        %p = nlinfit(x, y, f, p0);
        %K(x,y)=p(1)*L0^3+p(2)*L0^2+p(3)*L0+p(4);
        Z=Ks(i,j,:,:);
        [X,Y] = meshgrid(L0s,L0s);
        x=X(:);
        y=Y(:);
        z=Z(:);
        %x-l_r,y-l_l

        % f = fit([x, y],z,'poly22')
        % K(i,j)=f.p00 + f.p10*l_l + f.p01*l_r + f.p20*l_l^2 + f.p11*l_l*l_r + f.p02*l_r^2;
        
        f = fit([x, y],z,'poly33')
        K(i,j)=f.p00 + f.p10*l_r + f.p01*l_l ...
             + f.p20*l_r^2 + f.p11*l_r*l_l + f.p02*l_l^2 ...
             + f.p30*l_r^3 + f.p21*l_r^2*l_l + f.p12*l_r*l_l^2 + f.p03*l_l^3;
        
        % f = fit([x, y],z,'poly44')
        % K(i,j)=f.p00 + f.p10*l_r + f.p01*l_l ...
        %      + f.p20*l_r^2 + f.p11*l_r*l_l + f.p02*l_l^2 ...
        %      + f.p30*l_r^3 + f.p21*l_r^2*l_l + f.p12*l_r*l_l^2 + f.p03*l_l^3 ...
        %      + f.p40*l_r^4 + f.p31*l_r^3*l_l + f.p22*l_r^2*l_l^2 + f.p13*l_r*l_l^3 + f.p04*l_l^4;
    end
end

matlabFunction(K,'File','L2K');
